/**
 * @file pid.c
 * @brief 
 * 
 * @author 1024Tech (店铺链接：https://1024tech.taobao.com/)
 * @version 1.0
 * @date 2023-06-08
 * 
 * @copyright Copyright (c) 2023  Open Robot Tech.co, Ltd
 * 
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Version <th>Author  <th>Description
 * <tr><td>2023-05-08 <td>1.0     <td>Mr.lin     <td>Release
 * </table>
 */
#include "pid.h"

_PID_TYPE pidType;

/**
  * @brief   pid控制器参数，每行代表一个pid控制器参数
  */
const float  controller_parameter[6][5] =
{
    /* 0.kp 1.ki 2.kd 3.积分限幅  4.pid输出限幅值 */
    {0.0 , 0.0,  0.0,  500 , 1000 },    //外环速度环   
    {0.0 , 0.0,  0.0,  550 , 1000 },    //内环角度环       
    {0.0 , 0.0,  0.0,  500 , 1000 },    //内环角速度环

    {0.0 , 0.0,  0.0,  500 , 1000 },    //外环速度环   
    {0.0 , 0.0,  0.0,  550 , 1000 },    //内环角度环       
    {0.0 , 0.0,  0.0,  500 , 1000 },    //内环角速度环
};

/**
  * @brief   PID参数初始化配置
  * @param   *controller PID控制器指针，指向不同的控制器
  * @param   label PID参数标号
  */
static void _para_table(_PID *controller,uint8_t label)
{
    controller->kp              = controller_parameter[label][0];         
    controller->ki              = controller_parameter[label][1];         
    controller->kd              = controller_parameter[label][2];         
    controller->integral_max    = controller_parameter[label][3];         
    controller->out_max         = controller_parameter[label][4];               
}
/* PID参数初始化 */
void pid_init(void)
{
    _para_table(&pidType.rol_encoder,0);	
    _para_table(&pidType.rol_angle,1);
    _para_table(&pidType.rol_gyro,2);
	
    _para_table(&pidType.yaw_encoder,3);	
    _para_table(&pidType.yaw_angle,4);
    _para_table(&pidType.yaw_gyro,5);
} 

/**
  * @brief   PID控制器
  * @param   *controller PID控制器指针，指向不同的控制器
  * @param   enable_lowpass 偏差低通使能位：1使能，其他失能
  * @retval  controller->out 经过控制后的输出值
  */
float pid_controller(_PID *controller , uint8_t enable_lowpass)
{
#define LOWPASS_FACTOR 0.2F

    controller->err = controller->expect - controller->feedback; 
    if( 1 == enable_lowpass ){
        controller->err = controller->err*(1-LOWPASS_FACTOR) + controller->err_last*LOWPASS_FACTOR ;
    }
    controller->integral += controller->ki * controller->err;           
    /* 积分限幅 */
    if(controller->integral >  controller->integral_max) {
        controller->integral =  controller->integral_max;
    }
    if(controller->integral < -controller->integral_max) {
        controller->integral = -controller->integral_max;
    }
    /* pid运算 */
    controller->out =  controller->kp*controller->err + \
                       controller->integral + \
                       controller->kd*(controller->err-controller->err_last);
    /* 输出限幅 */
    if(controller->out >  controller->out_max) {
        controller->out =  controller->out_max;
    }
    if(controller->out < -controller->out_max) {
        controller->out = -controller->out_max;
    }
    controller->err_last = controller->err;                             
    return controller->out;
}
/**
  * @brief   PID控制器积分项清除
  * @param   *controller PID控制器指针，指向不同的控制器
  */
void clear_integral(_PID *controller)
{
    controller->integral = 0.0f;
}

